/*
 Copyright (C) 2010  ARToolkitPlus Authors

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.

 Authors:
 Daniel Wagner
 */

#include <ARToolKitPlus/Tracker.h>
#include <ARToolKitPlus/arBitFieldPattern.h>

#include <cassert>
#include <memory.h>

namespace ARToolKitPlus {

static void applyMaskSimple(IDPATTERN& nPattern) {
	nPattern ^= fullMask;
}

static void applyMaskBCH(IDPATTERN& nPattern) {
	nPattern ^= bchMask;
}

bool isBitSet(IDPATTERN pat, int which) {
	return ((pat >> which) & 1) != 0;
}

static void rotate90CW(IDPATTERN& nPattern) {
	IDPATTERN one = 1, tmpPat = nPattern;
	nPattern = 0;

	for (int i = 0; i < pattBits; i++) {
		if (isBitSet(tmpPat, rotate90[i]))
			nPattern |= (one << i);
	}
}

void generatePatternSimple(int nID, IDPATTERN& nPattern) {
	IDPATTERN tmpPat = nID & idMask;

	nPattern = (tmpPat << posMask0) | (tmpPat << posMask1) | (tmpPat << posMask2) | (tmpPat << posMask3);
	applyMaskSimple(nPattern);
}

void generatePatternBCH(int nID, IDPATTERN& nPattern) {
	BCH bchProcessor;
	_64bits encoded;

	bchProcessor.encode(encoded, nID);

	nPattern = encoded;
	applyMaskBCH(nPattern);
}

static float checkPatternBitSimple(IDPATTERN nPattern, int nBit, int& nValue) {
	int b0 = (int) (nPattern >> (posMask0 + nBit)) & 1, b1 = (int) (nPattern >> (posMask1 + nBit)) & 1, b2 =
			(int) (nPattern >> (posMask2 + nBit)) & 1, b3 = (int) (nPattern >> (posMask3 + nBit)) & 1, sum = b0 + b1
			+ b2 + b3;

	switch (sum) {
	case 0:
		nValue = 0;
		return 1.00f;
	case 1:
		nValue = 0;
		return 0.50f;
	case 2:
		nValue = 0; // opt for zero since one means white area which can happen due to reflectance
		return 0.00f;
	case 3:
		nValue = 1;
		return 0.50f;
	case 4:
		nValue = 1;
		return 1.00f;
	}

	// should never come here...
	//assert(false);
	return 0.0f;
}

static void checkPatternSimple(IDPATTERN nPattern, int& nID, float& nProp) {
	nProp = 0.0f;
	nID = 0;

	applyMaskSimple(nPattern);

	for (int i = 0; i < idBits; i++) {
		int bitvalue = 0;
		nProp += checkPatternBitSimple(nPattern, i, bitvalue);

		nID |= (bitvalue << i);
	}

	nProp /= (float) idBits;

	if (nProp < 0.9f)
		nProp = 0.0f;
}

static void checkPatternBCH(IDPATTERN nPattern, int& nID, float& nProp, BCH* nProcessor) {
	int err = -1;
	_64bits decodedPattern = 0;
	unsigned int andMask = (1 << bchBits) - 1;

	nProp = 0.0f;
	nID = 0;

	applyMaskBCH(nPattern);

	nProcessor->decode(err, decodedPattern, nPattern);

	nID = (int) (decodedPattern & andMask);

	switch (err) {
	case 0:
		nProp = 1.00f;
		break;

	case 1:
		nProp = 0.75f;
		break;

	case 2:
		nProp = 0.50f;
		break;

	case 3:
		nProp = 0.25f;
		break;

	default:
		nProp = 0.0f;
		break;
	}
}

int Tracker::downsamplePattern(uint8_t* data, unsigned char* imgPtr) {
	int x, y;

	if (PATTERN_WIDTH == 18 && PATTERN_HEIGHT == 18) {
		// this code simply takes the center pixel of each 3x3 cell
		// this is the fastest method for 18x18 to convert down to 6x6 but gives worst results
		// even more: this method gives no advantage over directly using a pattern size of 6x6.
		//
		/*
		 for(y=1; y<PATTERN_HEIGHT; y+=3)
		 for(x=1; x<PATTERN_WIDTH; x+=3)
		 {
		 int idx = (y*PATTERN_WIDTH+x)*3;
		 *imgPtr++ = (data[idx+0]+(data[idx+1]<<1)+data[idx+2])>>2;
		 }
		 */

		// this code downsamples the 18x18 RGB24 image to 6x6 LUM8 by
		// averaging each 3x3 cell into a single pixel
		//
		for (y = 0; y < PATTERN_HEIGHT; y += 3)
			for (x = 0; x < PATTERN_WIDTH; x += 3) {
				int idx = (y * PATTERN_WIDTH + x) * 3, val = 0;
				val = (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += PATTERN_WIDTH * 3 - 6;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += PATTERN_WIDTH * 3 - 6;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				*imgPtr++ = val / 9;
			}

		/*
		 FILE* fp = fopen("dump18x18.raw", "wb");
		 fwrite(data, PATTERN_HEIGHT*PATTERN_WIDTH*3, 1, fp);
		 fclose(fp);

		 fp = fopen("dump6x6.raw", "wb");
		 fwrite(patimg, 6*6, 1, fp);
		 fclose(fp);
		 */
	} else if (PATTERN_WIDTH == 12 && PATTERN_HEIGHT == 12) {
		// this code downsamples the 12x12 RGB24 image to 6x6 LUM8 by
		// averaging each 2x2 cell into a single pixel
		//
		for (y = 0; y < PATTERN_HEIGHT; y += 2)
			for (x = 0; x < PATTERN_WIDTH; x += 2) {
				int idx = (y * PATTERN_WIDTH + x) * 3, val = 0;
				val = (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += PATTERN_WIDTH * 3 - 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				idx += 3;
				val += (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

				*imgPtr++ = val / 4;
			}

		/*
		 FILE* fp = fopen("dump12x12.raw", "wb");
		 fwrite(data, PATTERN_HEIGHT*PATTERN_WIDTH*3, 1, fp);
		 fclose(fp);

		 fp = fopen("dump6x6.raw", "wb");
		 fwrite(patimg, 6*6, 1, fp);
		 fclose(fp);
		 */
	} else if (PATTERN_WIDTH == 6 && PATTERN_HEIGHT == 6) {
		// this code simply converts the 6x6 RGB pattern into a
		// 6x6 greyscale image
		//
		for (int idx = 0; idx < PATTERN_WIDTH * PATTERN_HEIGHT * 3; idx += 3)
			*imgPtr++ = (data[idx + 0] + (data[idx + 1] << 1) + data[idx + 2]) >> 2;

		/*
		 FILE* fp = fopen("dump6x6.raw", "wb");
		 fwrite(data, PATTERN_HEIGHT*PATTERN_WIDTH, 1, fp);
		 fclose(fp);
		 */
	} else {
		// the pattern size has to be 18x18, 12x12 or 6x6
		// for performance reasons generic downsampling is not supported
		assert((PATTERN_WIDTH==18 && PATTERN_HEIGHT==18) || (PATTERN_WIDTH==12 && PATTERN_HEIGHT==12) || (PATTERN_WIDTH==6 && PATTERN_HEIGHT==6));
		return -1;
	}

	return 0;
}

int Tracker::bitfield_check_simple(uint8_t *data, int *code, int *dir, ARFloat *cf, int thresh) {
	assert(sizeof(IDPATTERN)>=8 && "IDPATTERN must be at least 64-bit integer");

	unsigned char patimg[idPattWidth * idPattHeight], *imgPtr = patimg;
	int i;

	// first step is to reduce the pattern to 6x6. only the following
	// three resolutions are supported:
	//   - 18x18 (downsampling of each 3x3 cell)
	//   - 12x12 (downsampling of each 2x2 cell)
	//   -  6x6  (no downsampling)
	//
	if (downsamplePattern(data, imgPtr) == -1) {
		*code = 0;
		*dir = 0;
		*cf = -1.0f;
		return -1;
	}

	// now we do a thresholding and create the IDPATTERN bitfield
	//
	IDPATTERN pat = 0, one = 1;

	for (i = 0; i < pattBits; i++)
		if (patimg[pattBits - 1 - i] > thresh)
			pat |= one << i;

	// finally we check all four rotations and take the best one
	// if it is good enough
	//
	IDPATTERN pat0, pat90, pat180, pat270;
	int id0 = -1, id90 = -1, id180 = -1, id270 = -1;
	float prop0 = 0.0f, prop90 = 0.0f, prop180 = 0.0f, prop270 = 0.0f;

	pat0 = pat;
	checkPatternSimple(pat0, id0, prop0);

	pat90 = pat0;
	rotate90CW(pat90);
	checkPatternSimple(pat90, id90, prop90);

	pat180 = pat90;
	rotate90CW(pat180);
	checkPatternSimple(pat180, id180, prop180);

	pat270 = pat180;
	rotate90CW(pat270);
	checkPatternSimple(pat270, id270, prop270);

	if (prop0 >= prop90 && prop0 >= prop180 && prop0 >= prop270) // is prop0 maximum?
	{
		*dir = 0;
		*cf = prop0;
		*code = id0;
	} else if (prop90 >= prop0 && prop90 >= prop180 && prop90 >= prop270) // is prop90 maximum?
	{
		*dir = 1;
		*cf = prop90;
		*code = id90;
	} else if (prop180 >= prop0 && prop180 >= prop90 && prop180 >= prop270) // is prop180 maximum?
	{
		*dir = 2;
		*cf = prop180;
		*code = id180;
	} else if (prop270 >= prop0 && prop270 >= prop90 && prop270 >= prop180) // is prop270 maximum?
	{
		*dir = 3;
		*cf = prop270;
		*code = id270;
	} else {
		assert(false);
	}

	return 0;
}

int Tracker::bitfield_check_BCH(uint8_t *data, int *code, int *dir, ARFloat *cf, int thresh) {
	assert(sizeof(IDPATTERN)>=8 && "IDPATTERN must be at least 64-bit integer");

	unsigned char patimg[idPattWidth * idPattHeight], *imgPtr = patimg;
	int i;

	// first step is to reduce the pattern to 6x6. only the following
	// three resolutions are supported:
	//   - 18x18 (downsampling of each 3x3 cell)
	//   - 12x12 (downsampling of each 2x2 cell)
	//   -  6x6  (no downsampling)
	//
	if (downsamplePattern(data, imgPtr) == -1) {
		*code = 0;
		*dir = 0;
		*cf = -1.0f;
		return -1;
	}

	// now we do a thresholding and create the IDPATTERN bitfield
	//
	IDPATTERN pat = 0, one = 1;

	for (i = 0; i < pattBits; i++)
		if (patimg[pattBits - 1 - i] > thresh)
			pat |= one << i;

	// finally we check all four rotations and take the best one
	// if it is good enough
	//
	IDPATTERN pat0, pat90, pat180, pat270;
	int id0 = -1, id90 = -1, id180 = -1, id270 = -1;
	float prop0 = 0.0f, prop90 = 0.0f, prop180 = 0.0f, prop270 = 0.0f;

	if (bchProcessor == NULL)
		bchProcessor = new BCH;

	pat0 = pat;
	checkPatternBCH(pat0, id0, prop0, bchProcessor);

	pat90 = pat0;
	rotate90CW(pat90);
	checkPatternBCH(pat90, id90, prop90, bchProcessor);

	pat180 = pat90;
	rotate90CW(pat180);
	checkPatternBCH(pat180, id180, prop180, bchProcessor);

	pat270 = pat180;
	rotate90CW(pat270);
	checkPatternBCH(pat270, id270, prop270, bchProcessor);

	if (prop0 >= prop90 && prop0 >= prop180 && prop0 >= prop270) // is prop0 maximum?
	{
		*dir = 0;
		*cf = prop0;
		*code = id0;
	} else if (prop90 >= prop0 && prop90 >= prop180 && prop90 >= prop270) // is prop90 maximum?
	{
		*dir = 1;
		*cf = prop90;
		*code = id90;
	} else if (prop180 >= prop0 && prop180 >= prop90 && prop180 >= prop270) // is prop180 maximum?
	{
		*dir = 2;
		*cf = prop180;
		*code = id180;
	} else if (prop270 >= prop0 && prop270 >= prop90 && prop270 >= prop180) // is prop270 maximum?
	{
		*dir = 3;
		*cf = prop270;
		*code = id270;
	} else {
		assert(false);
	}

	return 0;
}

} // namespace ARToolKitPlus
